package javavis.jip3d.functions;

import java.io.FileWriter;
import java.io.IOException;
import java.util.ArrayList;

import javax.vecmath.Color3f;

import cern.colt.matrix.DoubleMatrix2D;
import cern.colt.matrix.impl.DenseDoubleMatrix2D;
import cern.colt.matrix.linalg.EigenvalueDecomposition;
import javavis.base.Function3DGroup;
import javavis.base.JIPException;
import javavis.base.ParamType;
import javavis.jip3d.geom.Feature2D;
import javavis.jip3d.geom.MyKDTree;
import javavis.jip3d.geom.MyTransform3D;
import javavis.jip3d.geom.Point3D;
import javavis.jip3d.geom.Quaternion;
import javavis.jip3d.geom.Segment3D;
import javavis.jip3d.gui.Function3D;
import javavis.jip3d.gui.FunctionParam;
import javavis.jip3d.gui.ScreenData;
import javavis.jip3d.gui.ScreenOptions;
import javavis.jip3d.gui.dataobjects.FeatureSet2D;
import javavis.jip3d.gui.dataobjects.SegmentSet3D;

public class FFastICPFeatures extends Function3D {
	/**
	 * @uml.property  name="error"
	 */
	double error;
	/**
	 * @uml.property  name="finalTransform"
	 * @uml.associationEnd  
	 */
	MyTransform3D finalTransform;
	/**
	 * @uml.property  name="finalMatches"
	 */
	int finalMatches;
	public final static double ANG_ERROR=0.5; // +-30degrees

	public FFastICPFeatures() {
		super();
		this.allowed_input = ScreenOptions.tFEATURESET2D;
		this.group = Function3DGroup.Egomotion;

		FunctionParam p1 = new FunctionParam("Next Object", ParamType.SCRDATA);
		FunctionParam p2 = new FunctionParam("Iterations", ParamType.INT);
		p2.setValue(1);
		FunctionParam p4 = new FunctionParam("dist_thresh", ParamType.FLOAT);
		p4.setValue(25000000.0f);
		FunctionParam p5 = new FunctionParam("max_dist", ParamType.FLOAT);
		p5.setValue(2.0f);
		FunctionParam p6 = new FunctionParam("min_dist", ParamType.FLOAT);
		p6.setValue(0.0f);
		FunctionParam p7 = new FunctionParam("max_move", ParamType.FLOAT);
		p7.setValue(1.0f);
		


		FunctionParam p8 = new FunctionParam("angle X", ParamType.FLOAT);
		p8.setValue(0.0);
		FunctionParam p9 = new FunctionParam("angle Y", ParamType.FLOAT);
		p9.setValue(0.0);
		FunctionParam p10 = new FunctionParam("angle Z", ParamType.FLOAT);
		p10.setValue(0.0);
		FunctionParam p11 = new FunctionParam("traslation X", ParamType.FLOAT);
		p11.setValue(0.0);
		FunctionParam p12 = new FunctionParam("traslation Y", ParamType.FLOAT);
		p12.setValue(0.0);
		FunctionParam p13 = new FunctionParam("traslation Z", ParamType.FLOAT);
		p13.setValue(0.0);
		FunctionParam p14 = new FunctionParam("fileWriter", ParamType.OBJECT);

		this.addParam(p1);
		this.addParam(p2);
		this.addParam(p4);
		this.addParam(p5);
		this.addParam(p6);
		this.addParam(p7);
		this.addParam(p8);
		this.addParam(p9);
		this.addParam(p10);
		this.addParam(p11);
		this.addParam(p12);
		this.addParam(p13);
		this.addParam(p14);
	}

	@Override
	public void proccessData(ScreenData scr_data) throws JIPException {
		result_list = new ArrayList<ScreenData>();
		int iterations = this.paramValueInt("Iterations");
		ScreenData scenedata = this.paramValueScrData("Next Object");
		double error_prev;
		double dist_thresh = this.paramValueReal("dist_thresh");
		double max_dist = this.paramValueReal("max_dist");
		double min_dist = this.paramValueReal("min_dist");
		double max_move = this.paramValueReal("max_move");
		double xang = this.paramValueReal("angle X");
		double yang = this.paramValueReal("angle Y");
		double zang = this.paramValueReal("angle Z");
		double xtrans = this.paramValueReal("traslation X");
		double ytrans = this.paramValueReal("traslation Y");
		double ztrans = this.paramValueReal("traslation Z");
		FileWriter fw = (FileWriter)this.paramValueObject("fileWriter");
		

		MyTransform3D tr3d, initialTrans=new MyTransform3D(xtrans, ytrans, ztrans, xang, yang, zang);
		MyTransform3D auxT=null;
    	Feature2D []sceneTr;
    	Object []scene;
    	ArrayList<Pair> closest, closestFirst=null;
    	int cont=0;

    	double prog_inc = 100.0/iterations;

    	scene = scenedata.elements();

    	tr3d = new MyTransform3D();
		cont = 0;
		error = Double.MAX_VALUE;
		// Apply the initial transformation. It is used for camera transformation from the robot coordinate system
		scene = applyTrans (scene, initialTrans);
		scr_data.applyTransform(initialTrans);
		do {
			cont++;
			error_prev = error;
			// Aplicamos la transformaci�n al conjunto de datos
			sceneTr = applyTrans (scene, tr3d);
			//buscamos los mas cercanos
			closest = findMatches(sceneTr, scr_data.getData(), dist_thresh, max_dist, min_dist, max_move);
			if (cont==1) 
				closestFirst=(ArrayList<Pair>)closest.clone();
			
			// If we do not get enough matches, wide the range and start from the begining
			if (closest.size() < 6) {
				// This means that no enough matches are available
				if (max_dist > 30) {
					System.out.println("No matches available");
					error=-1;
					break;
				}
				tr3d = new MyTransform3D();
				cont = 0;
				error = Double.MAX_VALUE;
				error_prev=0;
				max_dist += 1;
				continue;
			}
			
			//obtenemos la trasformacionn en funcion de los mas cercanos
			auxT = calcTrans (closest);
			//update total transformation
			tr3d.applyTransform(auxT);
			error = calcError (closest, tr3d);

			progress += prog_inc;
		} while (cont<iterations && Math.abs(error-error_prev)>0.01);
		// Write into a file the results
		try {
			fw.write("\n"+tr3d.toString());
			if (closest.size()>5) {
				// First, store features with matches
				fw.write(closest.size()+"\n");
				for (int i=0; i< closest.size(); i++) {
					fw.write(closest.get(i).p_scene.toString()+"\n");
				}
				// Then, the rest of features
				fw.write(sceneTr.length-closest.size()+"\n");
				for (int i=0; i< sceneTr.length; i++) {
					Feature2D f = (Feature2D)sceneTr[i];
					if (!existsFeature (f, closest)) {
						fw.write(f.toString()+"\n");
					}
				}
			}
			else {
				// None enough matches were found
				fw.write("0\n");
				fw.write(sceneTr.length+"\n");
				for (int i=0; i< sceneTr.length; i++) {
					fw.write(((Feature2D)sceneTr[i]).toString()+"\n");
				}
			}
		} catch (IOException e) {
			System.out.println("Error FastICPFeatures:"+e);
		}
		sceneTr = applyTrans (scene, tr3d);
		finalTransform=tr3d;
		System.out.println("Error="+error+" "+tr3d);

		FeatureSet2D resulttras = new FeatureSet2D(new ScreenOptions());
		resulttras.name = "ICPBasic";
		resulttras.scr_opt.width = 1;
		resulttras.scr_opt.global_color = true;
		resulttras.scr_opt.color = new Color3f(0,1,0);
		for(Feature2D point: sceneTr)
			resulttras.insert(point);
		result_list.add(resulttras);
		mostrarPares(closestFirst, "First Matching");
		mostrarPares(applyTransform(closest, auxT), "Last Matching");
	}
	
	boolean existsFeature (Feature2D f, ArrayList<Pair> closest) {
		for (Pair p : closest) {
			if (p.p_scene == f) {
				return true;
			}
		}
		return false;
	}
	
	ArrayList<Pair> applyTransform (ArrayList<Pair> closest, MyTransform3D tr3d) {
		Feature2D f2d;
		ArrayList<Pair> ret=new ArrayList<Pair>();
		
		for (Pair p : closest) {
			f2d=new Feature2D(p.p_scene);
			f2d.applyTransform(tr3d);
			ret.add(new Pair(p.p_model, f2d));
		}
		return ret;
	}
	
	double calcError (ArrayList<Pair> closest, MyTransform3D tr3d) {
		double er=0.0;
		Feature2D f2d;
		
		for (Pair p : closest) {
			f2d=new Feature2D(p.p_scene);
			f2d.applyTransform(tr3d);
			er=p.p_model.getDistance(f2d);
		}
		return er/closest.size();
	}

	public MyTransform3D calcTrans(ArrayList<Pair> pairs)
	{
		MyTransform3D ret;
		Quaternion quat;
		double [][]sigmapx = new double[3][3];
		double [][]mean = new double[3][3];
		double []delta = new double[3];
		double [][]Q = new double[4][4];
		double meanx, meany, meanz;
		int tam = pairs.size();
		double mean2x, mean2y, mean2z;
		Feature2D modelP, sceneP;
		double trace;
		DenseDoubleMatrix2D A;
		DoubleMatrix2D V;
		DoubleMatrix2D D;
		EigenvalueDecomposition EVD;
		Point3D meanM, meanS;
		int best;
		double value;

		meanx = meany = meanz = 0;
		mean2x = mean2y = mean2z = 0;
		sigmapx[0][0] = sigmapx[0][1] = sigmapx[0][2] = 0;
		sigmapx[1][0] = sigmapx[1][1] = sigmapx[1][2] = 0;
		sigmapx[2][0] = sigmapx[2][1] = sigmapx[2][2] = 0;

		for(Pair p: pairs)
		{
			modelP = p.p_model;
			sceneP = p.p_scene;
			meanx += sceneP.getX();
			meany += sceneP.getY();
			meanz += sceneP.getZ();
			mean2x += modelP.getX();
			mean2y += modelP.getY();
			mean2z += modelP.getZ();
			sigmapx[0][0]+=sceneP.getX()*modelP.getX();
			sigmapx[0][1]+=sceneP.getX()*modelP.getY();
			sigmapx[0][2]+=sceneP.getX()*modelP.getZ();
			sigmapx[1][0]+=sceneP.getY()*modelP.getX();
			sigmapx[1][1]+=sceneP.getY()*modelP.getY();
			sigmapx[1][2]+=sceneP.getY()*modelP.getZ();
			sigmapx[2][0]+=sceneP.getZ()*modelP.getX();
			sigmapx[2][1]+=sceneP.getZ()*modelP.getY();
			sigmapx[2][2]+=sceneP.getZ()*modelP.getZ();
		}

		meanx /= tam;
		meany /= tam;
		meanz /= tam;
		mean2x /= tam;
		mean2y /= tam;
		mean2z /= tam;
		mean[0][0] = meanx * mean2x; mean[0][1] = meanx * mean2y; mean[0][2] = meanx * mean2z;
		mean[1][0] = meany * mean2x; mean[1][1] = meany * mean2y; mean[1][2] = meany * mean2z;
		mean[2][0] = meanz * mean2x; mean[2][1] = meanz * mean2y; mean[2][2] = meanz * mean2z;

		for(int i=0;i<3;i++)
			for(int j=0;j<3;j++)
				sigmapx[i][j] = (sigmapx[i][j]- mean[i][j]) / tam  ;
		delta[0] = sigmapx[1][2] - sigmapx[2][1];
		delta[1] = sigmapx[2][0] - sigmapx[0][2];
		delta[2] = sigmapx[0][1] - sigmapx[1][0];

		trace = sigmapx[0][0] + sigmapx[1][1] + sigmapx[2][2];

		Q[0][0] = trace; Q[0][1] = delta[0]; Q[0][2] = delta[1]; Q[0][3] = delta[2];
		Q[1][0] = delta[0]; Q[1][1] = sigmapx[0][0] + sigmapx[0][0] - trace; Q[1][2] = sigmapx[0][1] + sigmapx[1][0]; Q[1][3] = sigmapx[0][2] + sigmapx[2][0];
		Q[2][0] = delta[1]; Q[2][1] = sigmapx[1][0] + sigmapx[1][0]; Q[2][2] = sigmapx[1][1] + sigmapx[1][1] - trace; Q[2][3] = sigmapx[1][2] + sigmapx[2][1];
		Q[3][0] = delta[2]; Q[3][1] = sigmapx[2][0] + sigmapx[0][2]; Q[3][2] = sigmapx[2][1] + sigmapx[1][2]; Q[3][3] = sigmapx[2][2] + sigmapx[2][2] - trace;
		A = new DenseDoubleMatrix2D(Q);
		EVD = new EigenvalueDecomposition(A);
		D = EVD.getD();
		V = EVD.getV();
		best = 0;
		value = D.getQuick(0, 0);
		//Look for the maximum eigenvalue
		for (int i=1; i<4; i++)
			if (D.getQuick(i, i)>value) {
				value = D.getQuick(i, i);
				best = i;
			}
		quat = new Quaternion(V.getQuick(0, best), V.getQuick(1, best), V.getQuick(2, best), V.getQuick(3, best), 0, 0, 0);

		ret =  quat.getTransform();

		meanS = new Point3D(meanx, meany, meanz);
		meanM = new Point3D(mean2x, mean2y, mean2z);
		meanS.applyTransform(ret);

		ret.setTranslation((meanM.subPoint(meanS)).getCoords());

		return ret;
	}
	
	public ArrayList<Pair> findMatches (Object[] scenetr, MyKDTree model, double dist_thresh, double max_dist, double min_dist, 
			double max_move) throws JIPException {
		Feature2D p_scene;
    	ArrayList<Pair> ret = new ArrayList<Pair>();
    	ArrayList<Pair> candidates = new ArrayList<Pair>();
    	int tam;
    	double dist, distMin, distMin2;
    	double mean = 0;
    	double var = 0;
    	double aux;
    	double[] lowk, uppk;
    	Object[] feat_range;
    	int indexMin;
    	
    	lowk=new double[3];
    	uppk=new double[3];
    	lowk[0]=lowk[1]=-Double.MAX_VALUE;
    	uppk[0]=uppk[1]=Double.MAX_VALUE;
    	lowk[2]=-max_dist-max_move;
    	uppk[2]=-min_dist;
		try {
			feat_range=model.range(lowk, uppk);
		} catch (Exception e1) {
			throw new JIPException(e1.getMessage());
		}

    	for(int cont=0;cont<scenetr.length;cont++) {
    		p_scene = (Feature2D)scenetr[cont];
    		if (-p_scene.getZ()>min_dist && -p_scene.getZ()<max_dist) {
    			indexMin=-1;
    			distMin=Double.MAX_VALUE;
    			distMin2=Double.MAX_VALUE;
    			for (int i=0; i<feat_range.length; i++) {
    				// Take advantage of the Laplacian is case of SURF
    				if (p_scene.type==2 && p_scene.laplacian!=((Feature2D)feat_range[i]).laplacian) continue; 
    				dist=p_scene.getDistanceDescriptor((Feature2D)feat_range[i]);
    				if (dist < distMin) {
    					if (distMin<distMin2) distMin2=distMin;
		    			indexMin=i;
		    			distMin=dist;
    				}
    				else if (dist < distMin2) distMin2=dist;
    			}
    			if (distMin < 0.36*distMin2 && distMin < dist_thresh) {
    				Pair p = new Pair((Feature2D)feat_range[indexMin], p_scene);
	    			candidates.add(p);
    				mean += p.getDistance();
    			}
    		}
    	}

    	tam = candidates.size();
    	mean /= tam;

    	for(Pair p:candidates) {
    		aux = p.getDistance() - mean;
    		var += aux * aux;
    	}
    	var /= tam;
    	var = Math.sqrt(var);

    	for (Pair p:candidates) { 
    		if (Math.abs(p.getDistance()-mean)<var) {
    			ret.add(p);
    		}
    	}
    	
    	

    	System.out.println("Matches="+ret.size());
    	finalMatches = ret.size();
    	return ret;
	}
	
	/**
	 * @return
	 * @uml.property  name="error"
	 */
	public double getError() {
		return error;
	}
	
	public MyTransform3D getTransform () {
		return finalTransform;
	}
	
	/**
	 * @return
	 * @uml.property  name="finalMatches"
	 */
	public int getFinalMatches () {
		return finalMatches;
	}

	private Feature2D[] applyTrans(Object[] source, MyTransform3D tr3d)
	{
		Feature2D []ret;
		int cont, tam;

		tam = source.length;
		ret = new Feature2D[tam];

		for(cont=0;cont<tam;cont++)
		{
			ret[cont] = new Feature2D((Feature2D)source[cont]);
			ret[cont].applyTransform(tr3d);
		}

		return ret;
	}

	/**
	 * @author  miguel
	 */
	private class Pair
	{
		/**
		 * @uml.property  name="p_model"
		 * @uml.associationEnd  
		 */
		public Feature2D p_model;
		/**
		 * @uml.property  name="p_scene"
		 * @uml.associationEnd  
		 */
		public Feature2D p_scene;
		private double dist;

		public Pair(Feature2D m, Feature2D s)
		{
			p_model = m;
			p_scene = s;
			dist = p_model.getDistance(p_scene);
		}
		
		public double getDistance () {
			return dist;
		}
	}

	private void mostrarPares(ArrayList<Pair> pairs, String name)
	{
		SegmentSet3D segments;
		Feature2D point;

		segments = new SegmentSet3D(new ScreenOptions());
		segments.name = name;

		for(Pair p: pairs)
		{
			point = new Feature2D(p.p_scene);
			segments.insert(new Segment3D(point, p.p_model));
		}

		result_list.add(segments);
	}

}
